using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using GarageGames.Torque.Core;
using GarageGames.Torque.MathUtil;



namespace GarageGames.Torque.T3D.RigidCollision
{
    public class RigidBody
    {
        float mass = 1.0f;


        #region Constructors

        public RigidBody()
        {
            _scene = null;
            _property.InverseMass = 1.0f;
            _property.InverseObjectInertia = Matrix.Identity;
            _property.CenterOfMass = new Vector3();
            _property.LinearDrag = _property.AngularDrag = 0.02f;
            _property.GravityScale = 1.0f;
            _property.ExternalForce = new Vector3();
            _property.ExternalTorque = new Vector3();

            _material.Restitution = 0.3f;
            _material.KineticFriction = 0.6f;
            _material.StaticFriction = 0.8f;

            _state.LinearPosition = new Vector3();
            _state.LinearMomentum = new Vector3();
            _state.LinearVelocity = new Vector3();
            _state.AngularPosition = Quaternion.Identity;
            _state.AngularMomentum = new Vector3();
            _state.AngularVelocity = new Vector3();

            _state.InverseWorldInertia = Matrix.Identity;

            _contacts = null;
            _frozen = false;
            _data = null;
        }

        #endregion


        #region Public properties, operators, constants, and enums

        public bool Frozen
        {
            get { return _frozen; }
            internal set { _frozen = value; }
        }



        public bool Kinetic
        {
            get { return _property.InverseMass < Epsilon.Value; }
            set { if (value) InverseMass = 0.0f; }
        }



        public bool Immovable
        {
            get { return _immovable; }
            set { if (value) Kinetic = true; _immovable = value; }
        }



        public float Mass
        {
            get { return _property.InverseMass > Epsilon.Value ? 1.0f / _property.InverseMass : 0.0f; } // zero as good as anything if immovable
            set
            {
                if (value < Epsilon.Value)
                    // treat as almost no mass, but not zero
                    _property.InverseMass = 1.0f / Epsilon.Value;
                else
                    _property.InverseMass = 1.0f / value;
            }
        }



        public float InverseMass
        {
            get { return _property.InverseMass; }
            set
            {
                if (value < Epsilon.Value)
                    // treat as 0 inv mass (immovable)
                    _property.InverseMass = 0.0f;
                else
                    _property.InverseMass = value;
            }
        }



        public Vector3 ExternalForce
        {
            get { return _property.ExternalForce; }
            set { _property.ExternalForce = value; }
        }



        public Vector3 ExternalTorque
        {
            get { return _property.ExternalTorque; }
            set { _property.ExternalTorque = value; }
        }



        public float GravityScale
        {
            get { return _property.GravityScale; }
            set { _property.GravityScale = value; }
        }



        public Vector3 Position
        {
            get { return _state.LinearPosition; }
        }



        public Quaternion AngularPosition
        {
            get { return _state.AngularPosition; }
        }



        public Matrix Transform
        {
            get
            {
                Matrix m;
                Matrix.CreateFromQuaternion(ref _state.AngularPosition, out m);
                m.Translation = _state.LinearPosition;
                return m;
            }
            set
            {
                _state.AngularPosition = Quaternion.CreateFromRotationMatrix(value);
                _state.AngularPosition.Normalize();
                _state.LinearPosition = value.Translation;

                _UpdateInertialTensor();

                //update center of mass
                _UpdateCenterOfMass();
            }
        }



        public Vector3 Velocity
        {
            get { return _state.LinearVelocity; }
            set
            {
                _state.LinearVelocity = value;
                _state.LinearMomentum = Mass * value;
            }
        }



        public Vector3 Momentum
        {
            get { return _state.LinearMomentum; }
            set { _state.LinearMomentum = value; _UpdateVelocity(); }
        }



        public Vector3 AngularVelocity
        {
            get { return _state.AngularVelocity; }
        }



        public Vector3 AngularMomentum
        {
            get { return _state.AngularMomentum; }
            set { _state.AngularMomentum = value; _UpdateVelocity(); }
        }



        public RigidContactConstraint ContactChain
        {
            get { return _contacts; }
        }



        public RigidMaterial Material
        {
            get { return _material; }
            set { _material = value; }
        }



        public Matrix InverseWorldTensor
        {
            get { return _state.InverseWorldInertia; }
        }



        public RigidScene RigidScene
        {
            get { return _scene; }
            internal set { _scene = value; }
        }



        public int ContactLevel
        {
            get { return _contactLevel; }
            set { _contactLevel = value; }
        }

        #endregion


        #region Public methods

        public void Integrate(float dt)
        {
            float angle = _state.AngularMomentum.Length();

            if (angle != 0.0f)
            {
                float sinCosAngle = angle * dt * 0.5f;
                float sinHalfAngle = (float)Math.Sin(sinCosAngle);
                sinHalfAngle *= 1.0f / angle;

                Quaternion dq = Quaternion.Identity;
                dq.W = (float)Math.Cos(sinCosAngle);
                dq.X = _state.AngularVelocity.X * sinHalfAngle;
                dq.Y = _state.AngularVelocity.Y * sinHalfAngle;
                dq.Z = _state.AngularVelocity.Z * sinHalfAngle;

                _state.AngularPosition = dq * _state.AngularPosition;
                _state.AngularPosition.Normalize();

                //rotate position around the center of mass
                Vector3 lp = _state.LinearPosition - _state.WorldCenterOfMass;
                MulQuat(dq, ref lp, out _state.LinearPosition);    //dq.mulP(lp, &linearPosition);
                _state.LinearPosition += _state.WorldCenterOfMass;
            }

            //update angular momentum
            _state.AngularMomentum += (_state.AngularMomentum * -_property.AngularDrag) + ExternalTorque * dt;

            //update linear position, momentum
            _state.LinearPosition = _state.LinearPosition + _state.LinearVelocity * dt;
            _state.LinearMomentum += (_state.LinearMomentum * -_property.LinearDrag) + ExternalForce * dt;

            //update dependent state vars
            _UpdateInertialTensor();
            _UpdateVelocity();
            _UpdateCenterOfMass();
        }



        public void IntegratePosition(float dt)
        {
            float angle = _state.AngularMomentum.Length();

            if (angle != 0.0f)
            {
                float sinCosAngle = angle * dt * 0.5f;
                float sinHalfAngle = (float)Math.Sin(sinCosAngle);
                sinHalfAngle *= 1.0f / angle;

                Quaternion dq = Quaternion.Identity;
                dq.W = (float)Math.Cos(sinCosAngle);
                dq.X = _state.AngularVelocity.X * sinHalfAngle;
                dq.Y = _state.AngularVelocity.Y * sinHalfAngle;
                dq.Z = _state.AngularVelocity.Z * sinHalfAngle;

                _state.AngularPosition = dq * _state.AngularPosition;
                _state.AngularPosition.Normalize();


                //rotate position around the center of mass
                Vector3 lp = _state.LinearPosition - _state.WorldCenterOfMass;
                MulQuat(dq, ref lp, out _state.LinearPosition);    //dq.mulP(lp, &linearPosition);
                _state.LinearPosition += _state.WorldCenterOfMass;
            }

            _state.LinearPosition += _state.LinearVelocity * dt;


            _UpdateInertialTensor();
            _UpdateCenterOfMass();
        }



        public void IntegrateMomentum(float dt)
        {
            _state.LinearMomentum += (_state.LinearMomentum * -_property.LinearDrag + ExternalForce) * dt;
            _state.AngularMomentum += (_state.AngularMomentum * -_property.AngularDrag + ExternalTorque) * dt;

            _UpdateVelocity();
        }



        public void ApplyForce(Vector3 r, Vector3 force)
        {
            if (Kinetic)
                return;

            // Position relative to the object position
            _property.ExternalForce += force;
            _property.ExternalTorque += Vector3.Cross(r, force);
        }



        public void ApplyImpulse(Vector3 r, Vector3 impulse)
        {
            if (Kinetic)
                return;

            // Position relative to the object position
            _state.LinearMomentum += impulse;

            //rotational momentum and velocity
            _state.AngularMomentum += Vector3.Cross(r, impulse);
            _UpdateVelocity();
        }



        public void SetCenterOfMass(ref Vector3 newCenter)
        {
            //set center of mass relative to the origin
            _property.CenterOfMass = newCenter;

            _UpdateCenterOfMass();
        }



        public void SetObjectInertia(ref Matrix mat)
        {
            Matrix.Invert(ref mat, out _property.InverseObjectInertia);
            _UpdateInertialTensor();
        }



        /// <summary>
        /// Set moment of inertia tensor to the box with given dimensions
        /// </summary>
        /// <param name="r">Dimensions of a box</param>
        public void SetObjectInertia(ref Vector3 r)
        {
            //rotational moment of inertia of a box
            float ot = mass / 12.0f;
            float a = r.X * r.X;
            float b = r.Y * r.Y;
            float c = r.Z * r.Z;

            Matrix objectInertia = Matrix.Identity;
            objectInertia.M11 = ot * (b + c);
            objectInertia.M22 = ot * (c + a);
            objectInertia.M33 = ot * (a + b);

            SetObjectInertia(ref objectInertia);
        }



        /// <summary>
        /// Set moment of inertia tensor to unit sphere
        /// </summary>
        public void SetObjectInertia()
        {
            Matrix objectInertia = Matrix.Identity;
            float radius = 1.0f;

            objectInertia.M11 = objectInertia.M22 = objectInertia.M33 =
                (0.4f * mass * radius * radius);

            SetObjectInertia(ref objectInertia);
        }



        public float GetIsNullTensor()
        {
            if (new Matrix() == _state.InverseWorldInertia)
                return 0.0f;

            return 1.0f;
        }



        public void SetNullTensor()
        {
            _property.InverseObjectInertia = _state.InverseWorldInertia = new Matrix();
        }



        public Vector3 GetPointVelocity(Vector3 r)
        {
            // Position relative to the object position
            return _state.LinearVelocity + Vector3.Cross(_state.AngularVelocity, r);
        }



        public float GetStopConstant(Vector3 r, Vector3 normal)
        {
            float rotStop = 0.0f;

            if (!Kinetic)
            {
                Vector3 nxr = Vector3.Cross(normal, r);
                Vector3 nxri;
                Vector3.Transform(ref nxr, ref _state.InverseWorldInertia, out nxri);
                rotStop = Vector3.Dot(Vector3.Cross(r, nxri), normal);
            }

            return _property.InverseMass + rotStop;
        }



        public void PushState()
        {
            _save = _state;
        }



        public void PopState()
        {
            _state = _save;
        }



        public void SetData(object data)
        {
            _data = data;
        }



        public T GetData<T>()
        {
            return (T)_data;
        }

        #endregion


        #region Private, protected, internal methods

        void _UpdateVelocity()
        {
            if (!Kinetic)
            {
                _state.LinearVelocity = _state.LinearMomentum * _property.InverseMass;
                Vector3.Transform(ref _state.AngularMomentum, ref _state.InverseWorldInertia, out _state.AngularVelocity);
            }
            //if (float.IsNaN(_state.AngularVelocity.X))
            //    System.Diagnostics.Debugger.Break();
        }



        void _UpdateInertialTensor()
        {
            //// Rotate inverse inertia tensor into world space
            Matrix qmat;
            Matrix.CreateFromQuaternion(ref _state.AngularPosition, out qmat);
            Matrix iv = qmat * _property.InverseObjectInertia;
            qmat = Matrix.Transpose(qmat);
            _state.InverseWorldInertia = iv * qmat;

        }



        void _UpdateCenterOfMass()
        {
            MulQuat(_state.AngularPosition, ref _property.CenterOfMass, out _state.WorldCenterOfMass);
            _state.WorldCenterOfMass += _state.LinearPosition;
        }



        internal void _SetContactChain(RigidContactConstraint chain)
        {
            _contacts = chain;
        }

        #endregion


        #region Private, protected, internal fields

        // rigid can belong to one and only one scene at a time
        RigidScene _scene;

        // rigid internal description
        RigidState _state;
        RigidProperties _property;
        RigidMaterial _material;

        // Contact chain, reset every "frame"
        RigidContactConstraint _contacts;

        // rigid scene shock step work variables
        internal RigidBody _queue;
        int _contactLevel;
        bool _frozen;
        bool _immovable;

        // User data
        object _data;

        // todo: make debug only?
        RigidState _save;

        #endregion

        void MulQuat(Quaternion quat, ref Vector3 p, out Vector3 r)
        {
            Quaternion qi = quat;
            Quaternion qv = new Quaternion(p, 0.0f);

            InvertQuat(ref qi);
            qv = (qi * qv) * quat;
            r = new Vector3(qv.X, qv.Y, qv.Z);
        }



        void InvertQuat(ref Quaternion quat)
        {
            float magnitude =
                quat.W * quat.W +
                quat.X * quat.X +
                quat.Y * quat.Y +
                quat.Z * quat.Z;

            if (magnitude == 1.0f)
            {
                quat.X = -quat.X;
                quat.Y = -quat.Y;
                quat.Z = -quat.Z;
            }
            else
            {
                if (magnitude == 0.0f)
                    magnitude = 1.0f;
                else
                    magnitude = 1.0f / magnitude;

                quat.W *= magnitude;
                quat.X *= -magnitude;
                quat.Y *= -magnitude;
                quat.Z *= -magnitude;
            }
        }

    }
}

